/* AUTHOR -> LYUBOMIR KOZLOVSKI */

#include "move_hack.h"
#include "SERIALAPI.h"
#include "mouse_sensor.h"
#include "lpc_types.h"
#include "math.h"

#define SPEED                   32
#define MIN_SPEED               16
#define MAX_SPEED               39
#define READING_X_90_POS        3544
#define READING_Y_90_POS        -101
#define READING_X_90_NEG        -3544
#define READING_Y_90_NEG        -101
#define ROT_SPEED               25
#define STEP                    2


/* Change P_TO_CM in mouse lib if dist not accurate */

void move_straight(uint8_t dist, double threshold)
{
    uint8_t flag_left = 0;
    uint8_t flag_right = 0;
    uint32_t t = 500000;               // time before first correction
    int32_t x_orig = get_x_cm();
    int32_t y_orig = get_y_cm();
    M1_FORWARD(SPEED);
    M2_FORWARD(SPEED);
    uint8_t m1 = SPEED, m2 = SPEED;     // motor speed values
    
    while(get_y_cm() - y_orig < dist)
    {
        /*  Delay correction */
        while(--t);
        t = 50;
        
        
        /* Auto correct */
        
        if(get_x_cm() - x_orig < -threshold && !flag_left)
        {
            flag_left = 1;
            m1 += STEP;
            m2 -= STEP;
        }
        else if(get_x_cm() - x_orig > threshold && !flag_right) 
        {
            flag_right = 1;
            m2 += STEP;
            m1 -= STEP;
        }
        else
        {
            flag_left = 0;
            flag_right = 0;
            m1 = SPEED;
            m2 = SPEED;
        }
        
        /* Control max speed */
        
        if (m1 > MAX_SPEED)
        {
            m1 = MAX_SPEED;
        }
        else if (m1 < MIN_SPEED)
        {
            m1 = MIN_SPEED;
        }
        if (m2 > MAX_SPEED)
        {
            m2 = MAX_SPEED;
        }
        else if (m2 < MIN_SPEED)
        {
            m2 = MIN_SPEED;
        }
        
            
        /* Start motors with corrected values */
            
        M1_FORWARD(m1);
        M2_FORWARD(m2);   
    }

/*  NEG VALUES MIRRORED     */

void rotate(int8_t degrees)
{
    if(degrees == 0) return;
    int32_t orig_x = get_x();
    int32_t orig_y = get_y();
    int32_t expected_x = 0;
    int32_t expected_y = 0;
    if(degrees > 0)
    {
       expected_x = (degrees/90)*READING_X_90_POS;
       expected_y = (degrees/90)*READING_Y_90_POS;
       M1_FORWARD(ROT_SPEED);
       M2_REVERSE(ROT_SPEED);
       while((get_x() - orig_x) < expected_x ||     // x increases
                (get_y() - orig_y) > expected_y);   // y decreases
    }
    else if(degrees < 0)
    {
        degrees = degrees * -1;
        expected_x = (degrees/90)*READING_X_90_NEG; 
        expected_y = (degrees/90)*READING_Y_90_NEG;
        M1_REVERSE(ROT_SPEED);
        M2_FORWARD(ROT_SPEED);
        while((get_x() - orig_x) < expected_x ||    // x decreases
                (get_y() - orig_y) > expected_y);   // y decreases
    }
    

    M1_STOP();
    M2_STOP();
}

void init_mouse_hack()
{
    init_mouse_sensors();
    SERIAL_INIT();
}

